#pragma once
#include <vector>
#include <Eigen/Dense>

class FixedStepInterpolator
{
public:
      // 初始化：起点、终点、总步数（duration_ms / 5ms）
      void init(const std::vector<double> &start,
                const std::vector<double> &end,
                int total_steps);

      // 获取第step步的插值结果（step从0到total_steps-1）
      std::vector<double> getPosition(int step) const;

      int totalSteps() const { return total_steps_; }

private:
      Eigen::MatrixXd coefficients_; // 插值系数
      int total_steps_;
};

void FixedStepInterpolator::init(
    const std::vector<double> &start,
    const std::vector<double> &end,
    int total_steps)
{
      total_steps_ = total_steps;
      int num_joints = start.size();
      coefficients_.resize(6, num_joints);

      for (int i = 0; i < num_joints; ++i)
      {
            double q0 = start[i];
            double q1 = end[i];

            // 五次多项式系数（固定步长简化版）
            coefficients_(0, i) = q0;              // a
            coefficients_(1, i) = 0;               // b
            coefficients_(2, i) = 0;               // c
            coefficients_(3, i) = 10 * (q1 - q0);  // d
            coefficients_(4, i) = -15 * (q1 - q0); // e
            coefficients_(5, i) = 6 * (q1 - q0);   // f
      }
}

std::vector<double> FixedStepInterpolator::getPosition(int step) const
{
      std::vector<double> positions(coefficients_.cols());
      double t = step / (double)(total_steps_ - 1); // 归一化到[0,1]

      for (int i = 0; i < coefficients_.cols(); ++i)
      {
            positions[i] = coefficients_(0, i) +
                           coefficients_(1, i) * t +
                           coefficients_(2, i) * t * t +
                           coefficients_(3, i) * t * t * t +
                           coefficients_(4, i) * t * t * t * t +
                           coefficients_(5, i) * t * t * t * t * t;
      }
      return positions;
}